{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

/* ----------------------------------------------------------------------------
 * SymForce - Copyright 2022, Skydio, Inc.
 * This source code is under the Apache 2.0 license found in the LICENSE file.
 * ---------------------------------------------------------------------------- */

/**
 * Tests for C++ geometry types. Mostly checking all the templates compile since
 * the math is tested comprehensively in symbolic form.
 */

#include <array>
#include <iostream>
#include <limits>

{% for cls in all_types %}
#include <sym/{{ camelcase_to_snakecase(cls.__name__) }}.h>
{% endfor %}

{% for cls in all_types %}
#include "symforce_function_codegen_test_data/symengine/symforce_gen_codegen_test_data/tangent_d_storage/tangent_d_storage_{{ camelcase_to_snakecase(cls.__name__) }}.h"
{% endfor %}

#include <sym/util/epsilon.h>
#include <symforce/opt/util.h>

#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>

TEST_CASE("Test Rot3", "[geo_package]") {
  // Make a random rotation
  std::mt19937 gen(42);
  const sym::Rot3f rot = sym::Rot3f::Random(gen);
  const double tol = 1e-6;

  // Cast
  const sym::Rot3d rotd = rot.Cast<double>();
  CHECK(sym::IsClose(rotd, rot.Cast<double>(), tol));
  CHECK(sym::IsClose(rotd.Cast<float>(), rot, tol));

  // Convert to Eigen rotation representations
  const Eigen::Quaternionf quat = rot.Quaternion();
  const Eigen::AngleAxisf aa = rot.AngleAxis();
  const Eigen::Matrix<float, 3, 3> mat = rot.ToRotationMatrix();
  const Eigen::Matrix<float, 3, 1> ypr = rot.ToYawPitchRoll();

  // Rotate a point
  const Eigen::Vector3f point = sym::Random<Eigen::Vector3f>(gen);
  CHECK((quat * point).isApprox(aa * point, 1e-6));
  CHECK((quat * point).isApprox(mat * point, 1e-6));
  CHECK((quat * point).isApprox(rot * point, 1e-6));

  // Rotate a point as an initializer expression
  CHECK((quat * Eigen::Vector3f::UnitX()).isApprox(rot * Eigen::Vector3f::UnitX(), 1e-6));

  // Construct back from Eigen rotation representations
  CHECK(sym::IsClose(sym::Rot3f(quat), rot, tol));
  CHECK(sym::IsClose(sym::Rot3f(aa), rot, tol));
  CHECK(sym::IsClose(sym::Rot3f::FromRotationMatrix(mat), rot, tol));
  CHECK(sym::IsClose(sym::Rot3f::FromYawPitchRoll(ypr).ToPositiveReal(), rot, tol));

  // Make a pose
  const sym::Pose3f pose(sym::Rot3f(aa), point);
  CHECK(sym::IsClose(pose.Rotation(), rot, tol));
  CHECK(pose.Position() == point);

  const sym::Pose3f pose_inv = pose.Inverse();
  CHECK(sym::IsClose(pose_inv.Rotation(), rot.Inverse(), tol));

  // TODO(bradley): Figure out why this fails if we iterate 1000 times and fix it
  // Transform a point with a pose
  CHECK((pose_inv * point).norm() < 1e-6);

  // Transform a point as an initializer expression
  CHECK((pose * Eigen::Vector3f::UnitX().eval()).isApprox(pose * Eigen::Vector3f::UnitX(), 1e-6));

  // Check that the log returns vectors with norm less than pi, and is the inverse of exp
  for (int i = 0; i < 1000; i++) {
    const sym::Rot3d rot = sym::Rot3d::Random(gen);
    const Eigen::Vector3d log = rot.ToTangent();
    CHECK(log.norm() <= M_PI);
    const sym::Rot3d exp_log_rot = sym::Rot3d::FromTangent(log);

    // The quaternion might not be equal, it might be negated, but the matrix should be equal
    CHECK(rot.ToRotationMatrix().isApprox(exp_log_rot.ToRotationMatrix(), 1e-9));
  }
}

TEST_CASE("Test Pose3", "[geo_package]") {
  // Make a random pose
  std::mt19937 gen(42);
  const sym::Pose3d pose = sym::Random<sym::Pose3d>(gen);

  // Test InverseCompose
  const Eigen::Vector3d point = sym::Random<Eigen::Vector3d>(gen);
  CHECK(pose.InverseCompose(point).isApprox(pose.Inverse() * point, 1e-9));
}

TEST_CASE("Test Rot2 and Pose2", "[geo_package]") {
  std::mt19937 gen(42);
  const sym::Rot2f rot = sym::Random<sym::Rot2f>(gen);
  const Eigen::Vector2f pos = sym::Random<Eigen::Vector2f>(gen);

  // Cast
  const sym::Rot2d rotd = rot.Cast<double>();
  CHECK(rotd.IsApprox(rot.Cast<double>(), sym::kDefaultEpsilon<float>));
  CHECK(rotd.Cast<float>().IsApprox(rot, sym::kDefaultEpsilon<float>));

  // Make a pose
  const sym::Pose2f pose(rot, pos);
  CHECK(pose.Rotation().IsApprox(rot, sym::kDefaultEpsilon<float>));
  CHECK(pose.Position() == pos);

  const sym::Pose2f pose_inv = pose.Inverse();
  CHECK(pose_inv.Rotation().IsApprox(rot.Inverse(), sym::kDefaultEpsilon<float>));

  // Test InverseCompose
  const Eigen::Vector2f point = sym::Random<Eigen::Vector2f>(gen);
  CHECK(pose.InverseCompose(point).isApprox(pose.Inverse() * point, sym::kDefaultEpsilon<float>));

  // Test FromAngle and angle constructor
  const float angle = rot.ToTangent()(0);
  CHECK(rot.IsApprox(sym::Rot2f(angle), sym::kDefaultEpsilon<float>));
  CHECK(rot.IsApprox(sym::Rot2f::FromAngle(angle), sym::kDefaultEpsilon<float>));
}

TEMPLATE_TEST_CASE("Test Storage ops", "[geo_package]", {{ ", ".join(cpp_geo_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;

  std::mt19937 gen(24362);
  const T value = sym::Random<T>(gen);
  INFO("Testing StorageOps: " << value);

  constexpr int32_t storage_dim = sym::StorageOps<T>::StorageDim();
  CHECK(value.Data().rows() == storage_dim);
  CHECK(value.Data().cols() == 1);

  std::vector<Scalar> vec;
  vec.resize(storage_dim);
  sym::StorageOps<T>::ToStorage(value, vec.data());
  CHECK(vec.size() > 0);
  CHECK(static_cast<int>(vec.size()) == storage_dim);
  for (int i = 0; i < static_cast<int>(vec.size()); ++i) {
    CHECK(vec[i] == value.Data()[i]);
  }

  const T value2 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value.Data() == value2.Data());
  vec[0] = 2.1;
  vec[vec.size() - 1] = 1.2;
  const T value3 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value.Data() != value3.Data());
}

TEMPLATE_TEST_CASE("Test Scalar storage ops", "[geo_package]", {{ ", ".join(scalar_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;

  const T value{};
  INFO("Testing StorageOps: " << value);

  constexpr int32_t storage_dim = sym::StorageOps<T>::StorageDim();
  CHECK(storage_dim == 1);

  std::vector<Scalar> vec;
  vec.resize(storage_dim);
  sym::StorageOps<T>::ToStorage(value, vec.data());
  CHECK(vec.size() == storage_dim);
  CHECK(vec[0] == value);

  const T value2 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value == value2);
  vec[0] = 2.1;
  const T value3 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value != value3);
}

TEMPLATE_TEST_CASE("Test Matrix storage ops", "[geo_package]", {{ ", ".join(cpp_matrix_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;

  const T value = T::Zero();
  INFO("Testing Matrix StorageOps: " << value.transpose());

  constexpr int32_t storage_dim = sym::StorageOps<T>::StorageDim();
  CHECK(storage_dim == T::RowsAtCompileTime);

  std::vector<Scalar> vec;
  vec.resize(storage_dim);
  sym::StorageOps<T>::ToStorage(value, vec.data());
  CHECK(static_cast<int>(vec.size()) == storage_dim);
  for (int i = 0; i < static_cast<int>(vec.size()); ++i) {
    CHECK(vec[i] == value[i]);
  }

  const T value2 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value == value2);
  vec[0] = 2.1;
  const T value3 = sym::StorageOps<T>::FromStorage(vec.data());
  CHECK(value != value3);
}

TEST_CASE("Test Matrix storage order is consistent with symbolic storage order") {
  const auto m = (Eigen::Matrix3d() <<
    1.0, 2.0, 3.0,
    4.0, 5.0, 6.0,
    7.0, 8.0, 9.0
  ).finished();

  {% set symbolic_m = Matrix([
    [1.0, 2.0, 3.0],
    [4.0, 5.0, 6.0],
    [7.0, 8.0, 9.0],
  ])%}

  std::array<double, sym::StorageOps<Eigen::Matrix3d>::StorageDim()> storage;
  sym::StorageOps<Eigen::Matrix3d>::ToStorage(m, storage.data());

  const std::array<double, {{ symbolic_m.storage_dim() }}> symbolic_storage = {
    {% for val in symbolic_m.to_storage() %}
    {{ val }}{{", " if not loop.last else ""}}
    {% endfor %}
  };

  CHECK(storage == symbolic_storage);
}

TEMPLATE_TEST_CASE("Test Group ops", "[geo_package]", {{ ", ".join(cpp_group_geo_types) }}) {
  using T = TestType;
  using Scalar = typename sym::StorageOps<T>::Scalar;

  const T identity{};
  INFO("Testing GroupOps: " << identity);

  // TODO(hayk): Make sym::StorageOps<T>::IsApprox that uses ToStorage to compare, then
  // get rid of the custom scalar version below.
  CHECK(identity.IsApprox(sym::GroupOps<T>::Identity(), sym::kDefaultEpsilon<Scalar>));
  CHECK(identity.IsApprox(sym::GroupOps<T>::Compose(identity, identity),
                          sym::kDefaultEpsilon<Scalar>));
  CHECK(identity.IsApprox(sym::GroupOps<T>::Inverse(identity), sym::kDefaultEpsilon<Scalar>));
  CHECK(identity.IsApprox(sym::GroupOps<T>::Between(identity, identity),
                          sym::kDefaultEpsilon<Scalar>));
}

TEMPLATE_TEST_CASE("Test Scalar group ops", "[geo_package]", {{ ", ".join(scalar_types) }}) {
  using T = TestType;

  const T identity{};
  INFO("Testing GroupOps: " << identity);

  CHECK(identity == sym::GroupOps<T>::Identity());
  CHECK(identity == sym::GroupOps<T>::Compose(identity, identity));
  CHECK(identity == sym::GroupOps<T>::Inverse(identity));
  CHECK(identity == sym::GroupOps<T>::Between(identity, identity));
}

TEMPLATE_TEST_CASE("Test Matrix group ops", "[geo_package]", {{ ", ".join(cpp_matrix_types) }}) {
  using T = TestType;

  const T identity = T::Zero();
  INFO("Testing Matrix GroupOps: " << identity.transpose());

  CHECK(identity == sym::GroupOps<T>::Identity());
  CHECK(identity == sym::GroupOps<T>::Compose(identity, identity));
  CHECK(identity == sym::GroupOps<T>::Inverse(identity));
  CHECK(identity == sym::GroupOps<T>::Between(identity, identity));
}

// Returns a random element e which is not near the provided element v.
// "Not near" means the tangent length of v to e, vector is greater than tol.
// epsilon is to avoid singularies when checking the tangent vector.
// An example type for Generator is std::mt19937
template <typename T, typename Generator, typename Scalar>
T RandomNotNear(const T& v, Generator& gen, Scalar tol) {
  T e = sym::StorageOps<T>::Random(gen);
  // Making sure that element is not near identity, and trying again if it is.
  while (sym::LieGroupOps<T>::LocalCoordinates(v, e, sym::kDefaultEpsilon<Scalar>).norm() <
         2 * tol) {
    e = sym::StorageOps<T>::Random(gen);
  }
  return e;
}

// Tests that sym::LieGroupOps<T>::IsClose returns true when small tangent perturbations are
// applied.
template <typename T, typename Generator, typename Scalar>
void IsCloseCommonTest(Generator& gen, Scalar tol) {
  // Repeat code 100 times to get a better sampling
  for (int i = 0; i < 100; i++) {
    const T element = sym::StorageOps<T>::Random(gen);

    // Test the elements far away from each other are not close.
    const T different_element = RandomNotNear(element, gen, tol);
    CHECK(!sym::LieGroupOps<T>::IsClose(element, different_element, tol));

    // Test that close elements are close
    const T close_element = [&element, tol] {
      using TangentVec = typename sym::LieGroupOps<T>::TangentVec;
      TangentVec small_tangent = TangentVec::Random();
      if (small_tangent(0) == 0.0) {
        small_tangent(0) = 1.0;
      }
      // multiply by 0.8 so the perturbation has norm < tol
      small_tangent = (0.8 * tol / small_tangent.norm()) * small_tangent;
      return sym::LieGroupOps<T>::Retract(element, small_tangent, sym::kDefaultEpsilon<Scalar>);
    }();
    CHECK(sym::IsClose(element, close_element, tol));
  }
}

template <typename T>
struct TestIsClose {
  using Scalar = typename sym::StorageOps<T>::Scalar;

  // Test sym::LieGroupOps<T>::IsClose(a, b, tol). Intended usage is to simply call
  // TestIsClose<T>::Test(gen)
  // An example type for Generator is std::mt19937
  template <typename Generator>
  static void Test(Generator& gen, Scalar tol = 1e-6) {
    IsCloseCommonTest<T>(gen, tol);
  }
};

template <typename Scalar>
struct TestIsClose<sym::Rot3<Scalar>> {
  using Rot3 = sym::Rot3<Scalar>;

  template <typename Generator>
  static void Test(Generator& gen, const Scalar tol = 1e-6) {
    IsCloseCommonTest<sym::Rot3<Scalar>>(gen, tol);

    for (int i = 0; i < 50; i++) {
      // Test that that if one Rot3 is represented by quaternion q and another
      // Rot3 by -q, then IsClose returns true when called on those Rot3s.
      const Rot3 rot = Rot3::Random(gen);
      const Rot3 rot_negated(-rot.Data());
      CHECK(sym::LieGroupOps<Rot3>::IsClose(rot, rot_negated, tol));
    }

    // Test IsClose returns true when called on rotations with the same axis but
    // whose angles of rotation are pi + epsilon and pi - epsilon respectively.
    // (This test exists because many functions have a discontinuity at pi radians
    // as the w component of the quaternion representation of the quaternion changes
    // sign at that angle).
    using DataVec = typename Rot3::DataVec;
    const Eigen::Matrix<Scalar, 3, 1> xyz(1, -1, 2);  // The direction of xyz is arbitrary
    const Scalar w = 1e-7;
    const Rot3 rot_pi_plus((DataVec() << xyz, w).finished());
    const Rot3 rot_pi_minus((DataVec() << xyz, -w).finished());
    CHECK(sym::LieGroupOps<Rot3>::IsClose(rot_pi_plus, rot_pi_minus, tol));
  }
};

template <typename Scalar>
struct TestIsClose<sym::Rot2<Scalar>> {
  using Rot2 = sym::Rot2<Scalar>;

  template <typename Generator>
  static void Test(Generator& gen, Scalar tol = 10 * sym::kDefaultEpsilon<Scalar>) {
    IsCloseCommonTest<Rot2>(gen, tol);

    // Checking that IsClose returns true for angles which are close but on opposite sides
    // of potentially significant angles (ex: pi + epsilon and pi - epsilon), or when the angles
    // are close to each other mod 2 pi (ex: 2pi - epsilon and epsilon)
    const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;
    for (Scalar angle = -2 * M_PI; angle < 2 * M_PI + 0.1; angle += M_PI_2) {
      CHECK(sym::LieGroupOps<Rot2>::IsClose(Rot2(angle - epsilon), Rot2(angle + epsilon), tol));
    }
    for (Scalar angle : {-M_PI, 0.0}) {
      CHECK(sym::LieGroupOps<Rot2>::IsClose(Rot2(angle + epsilon), Rot2(2 * M_PI + angle - epsilon),
                                            tol));
    }
  }
};

template <typename Scalar>
struct TestIsClose<sym::Unit3<Scalar>> {
  using Unit3 = sym::Unit3<Scalar>;
  using TangentVec = typename sym::LieGroupOps<Unit3>::TangentVec;

  template <typename Generator>
  static void Test(Generator& gen, Scalar tol = 10 * sym::kDefaultEpsilon<Scalar>) {
    IsCloseCommonTest<Unit3>(gen, tol);

    // Test that IsClose returns true for tangent perturbations that are 2pi apart.
    const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;
    for (int i = 0; i < 100; i++) {
      const Unit3 unit = Unit3::Random(gen);
      // Perturb by 2pi in each tangent space direction.
      TangentVec perturbation = TangentVec::Zero();
      perturbation(0) = 2 * M_PI;
      const Unit3 unit_2pi_x = sym::LieGroupOps<Unit3>::Retract(unit, perturbation, epsilon);
      CHECK(sym::LieGroupOps<Unit3>::IsClose(unit, unit_2pi_x, tol));
      perturbation(0) = 0;
      perturbation(1) = 2 * M_PI;
      const Unit3 unit_2pi_y = sym::LieGroupOps<Unit3>::Retract(unit, perturbation, epsilon);
      CHECK(sym::LieGroupOps<Unit3>::IsClose(unit, unit_2pi_y, tol));
    }
  }
};

TEMPLATE_TEST_CASE("Test Lie group ops", "[geo_package]", {{ ", ".join(cpp_group_geo_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;
  using TangentVec = Eigen::Matrix<Scalar, sym::LieGroupOps<T>::TangentDim(), 1>;
  constexpr const int32_t storage_dim = sym::StorageOps<T>::StorageDim();
  using StorageVec = Eigen::Matrix<Scalar, storage_dim, 1>;
  using SelfJacobian = typename sym::GroupOps<T>::SelfJacobian;
  const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;

  const T identity = sym::GroupOps<T>::Identity();
  INFO("Testing LieGroupOps: " << identity);

  constexpr int32_t tangent_dim = sym::LieGroupOps<T>::TangentDim();
  CHECK(tangent_dim > 0);
  CHECK(tangent_dim <= sym::StorageOps<T>::StorageDim());

  // Checking that std::numeric_limits<Scalar>::epsilon() is sufficiently large to
  // avoid nan in ToTangent (used for IsClose). At the time of writing, only
  // Rot3 and Pose3 encounter issues with nan, and that is when we have the identity rotation.
  const TangentVec identity_tangent = identity.ToTangent(std::numeric_limits<Scalar>::epsilon());
  CHECK(!identity_tangent.array().isNaN().any());

  std::mt19937 gen(24362);
  TestIsClose<T>::Test(gen);

  const TangentVec perturbation = sym::Random<TangentVec>(gen);
  const T value = sym::LieGroupOps<T>::FromTangent(perturbation, epsilon);

  const TangentVec recovered_perturbation = sym::LieGroupOps<T>::ToTangent(value, epsilon);
  CHECK(perturbation.isApprox(recovered_perturbation, std::sqrt(epsilon)));

  const T recovered_identity = sym::LieGroupOps<T>::Retract(
    value, -recovered_perturbation, epsilon);
  CHECK(recovered_identity.IsApprox(identity, std::sqrt(epsilon)));

  const TangentVec perturbation_zero = sym::LieGroupOps<T>::LocalCoordinates(
    identity, recovered_identity, epsilon);
  CHECK(perturbation_zero.norm() < std::sqrt(epsilon));

  const T identity_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 0.0f, epsilon);
  CHECK(identity_interp.IsApprox(identity, std::sqrt(epsilon)));

  const T value_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 1.0f, epsilon);
  CHECK(value_interp.IsApprox(value, std::sqrt(epsilon)));

  SelfJacobian inverse_jacobian;
  sym::GroupOps<T>::InverseWithJacobian(identity, &inverse_jacobian);
  CHECK(inverse_jacobian.isApprox(-SelfJacobian::Identity(), epsilon));

  // Test perturbing one axis at a time by sqrt(epsilon)
  // Makes sure special cases of one-axis perturbations are handled correctly, and that distortion
  // due to epsilon doesn't extend too far away from 0
  {
    TangentVec small_perturbation = TangentVec::Zero();
    for (int i = 0; i < sym::LieGroupOps<T>::TangentDim(); i++) {
      small_perturbation(i) = std::sqrt(epsilon);
      const T value = sym::LieGroupOps<T>::FromTangent(small_perturbation, epsilon);
      const TangentVec recovered_perturbation = sym::LieGroupOps<T>::ToTangent(value, epsilon);
      CHECK(small_perturbation.isApprox(recovered_perturbation, 10 * epsilon));
      small_perturbation(i) = 0;
    }
  }

  // Test tangent_D_storage generated from the symbolic version in SymForce against numerical
  // derivatives
  for (size_t i = 0; i < 10000; i++) {
    const T a = sym::Random<T>(gen);

    StorageVec storage;
    sym::StorageOps<T>::ToStorage(a, storage.data());
    const Eigen::Matrix<Scalar, tangent_dim, storage_dim> numerical_tangent_D_storage =
        sym::NumericalDerivative(
            [epsilon, &a](const StorageVec& storage_perturbed) {
              return sym::LieGroupOps<T>::LocalCoordinates(
                  a, sym::StorageOps<T>::FromStorage(storage_perturbed.data()), epsilon);
            },
            storage, epsilon, std::sqrt(epsilon));

    const Eigen::Matrix<Scalar, tangent_dim, storage_dim> symforce_tangent_D_storage =
        sym::TangentDStorage(a, epsilon);

    CHECK(
        numerical_tangent_D_storage.isApprox(symforce_tangent_D_storage, 10 * std::sqrt(epsilon)));
  }

  // Test ComposeWithJacobians against numerical derivatives
  for (size_t i = 0; i < 10000; i++) {
    const T a = sym::Random<T>(gen);
    const T b = sym::Random<T>(gen);

    const SelfJacobian numerical_jacobian = sym::NumericalDerivative(
        std::bind(&sym::GroupOps<T>::Compose, std::placeholders::_1, b), a,
        epsilon, std::sqrt(epsilon));

    SelfJacobian symforce_jacobian;
    sym::GroupOps<T>::ComposeWithJacobians(a, b, &symforce_jacobian, nullptr);

    CHECK(numerical_jacobian.isApprox(symforce_jacobian, 10 * std::sqrt(epsilon)));
  }
}

// Similar test to Test Lie group ops, but without group specific operations (e.g. Identity, Compose, FromTangent, etc.)
TEMPLATE_TEST_CASE("Test Manifold ops", "[geo_package]", {{ ", ".join(cpp_geo_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;
  using TangentVec = Eigen::Matrix<Scalar, sym::LieGroupOps<T>::TangentDim(), 1>;
  constexpr const int32_t storage_dim = sym::StorageOps<T>::StorageDim();
  using StorageVec = Eigen::Matrix<Scalar, storage_dim, 1>;
  const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;

  std::mt19937 gen(24362);
  TestIsClose<T>::Test(gen);
  const T a = sym::Random<T>(gen);
  INFO("Testing LieGroupOps: " << a);

  constexpr int32_t tangent_dim = sym::LieGroupOps<T>::TangentDim();
  CHECK(tangent_dim > 0);
  CHECK(tangent_dim <= sym::StorageOps<T>::StorageDim());

  // Checking that std::numeric_limits<Scalar>::epsilon() is sufficiently large to
  // avoid nan in LocalCoordinates (used for IsClose). At the time of writing, only
  // Rot3 and Pose3 encounter issues with nan, and that is when we have the identity rotation.
  const TangentVec a_tangent = sym::LieGroupOps<T>::LocalCoordinates(a, a, std::numeric_limits<Scalar>::epsilon());
  CHECK(!a_tangent.array().isNaN().any());

  const TangentVec perturbation = sym::Random<TangentVec>(gen);
  const T b = sym::LieGroupOps<T>::Retract(a, perturbation, epsilon);

  const TangentVec recovered_perturbation = sym::LieGroupOps<T>::LocalCoordinates(a, b, epsilon);
  CHECK(perturbation.isApprox(recovered_perturbation, std::sqrt(epsilon)));

  const TangentVec reverse_perturbation = sym::LieGroupOps<T>::LocalCoordinates(b, a, epsilon);
  const T recovered_a = sym::LieGroupOps<T>::Retract(
    b, reverse_perturbation, epsilon);
  CHECK(recovered_a.IsApprox(a, std::sqrt(epsilon)));

  const TangentVec perturbation_zero = sym::LieGroupOps<T>::LocalCoordinates(
    a, recovered_a, epsilon);
  CHECK(perturbation_zero.norm() < std::sqrt(epsilon));

  const T a_interp = sym::LieGroupOps<T>::Interpolate(a, b, 0.0f, epsilon);
  CHECK(a_interp.IsApprox(a, std::sqrt(epsilon)));

  const T b_interp = sym::LieGroupOps<T>::Interpolate(a, b, 1.0f, epsilon);
  CHECK(b_interp.IsApprox(b, std::sqrt(epsilon)));

  // Test perturbing one axis at a time by sqrt(epsilon)
  // Makes sure special cases of one-axis perturbations are handled correctly, and that distortion
  // due to epsilon doesn't extend too far away from 0
  {
    TangentVec small_perturbation = TangentVec::Zero();
    for (int i = 0; i < sym::LieGroupOps<T>::TangentDim(); i++) {
      small_perturbation(i) = std::sqrt(epsilon);
      const T b = sym::LieGroupOps<T>::Retract(a, small_perturbation, epsilon);
      const TangentVec recovered_perturbation = sym::LieGroupOps<T>::LocalCoordinates(a, b, epsilon);
      CHECK((small_perturbation - recovered_perturbation).norm() <= 10 * epsilon);
      small_perturbation(i) = 0;
    }
  }

  // Test tangent_D_storage generated from the symbolic version in SymForce against numerical
  // derivatives
  for (size_t i = 0; i < 10000; i++) {
    const T a = sym::Random<T>(gen);

    StorageVec storage;
    sym::StorageOps<T>::ToStorage(a, storage.data());
    const Eigen::Matrix<Scalar, tangent_dim, storage_dim> numerical_tangent_D_storage =
        sym::NumericalDerivative(
            [epsilon, &a](const StorageVec& storage_perturbed) {
              return sym::LieGroupOps<T>::LocalCoordinates(
                  a, sym::StorageOps<T>::FromStorage(storage_perturbed.data()), epsilon);
            },
            storage, epsilon, std::sqrt(epsilon));

    const Eigen::Matrix<Scalar, tangent_dim, storage_dim> symforce_tangent_D_storage =
        sym::TangentDStorage(a, epsilon);

    CHECK(
        numerical_tangent_D_storage.isApprox(symforce_tangent_D_storage, 10 * std::sqrt(epsilon)));
  }
}

TEMPLATE_TEST_CASE("Test Scalar Lie group ops", "[geo_package]", {{ ", ".join(scalar_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;
  using TangentVec = Eigen::Matrix<Scalar, sym::LieGroupOps<T>::TangentDim(), 1>;
  const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;

  const T identity = sym::GroupOps<T>::Identity();
  INFO("Testing LieGroupOps: " << identity);

  constexpr int32_t tangent_dim = sym::LieGroupOps<T>::TangentDim();
  CHECK(tangent_dim > 0);
  CHECK(tangent_dim <= sym::StorageOps<T>::StorageDim());
  
  std::mt19937 gen(42);
  const TangentVec perturbation = sym::Random<TangentVec>(gen);
  const T value = sym::LieGroupOps<T>::FromTangent(perturbation, epsilon);

  TestIsClose<T>::Test(gen);

  const TangentVec recovered_perturbation = sym::LieGroupOps<T>::ToTangent(value, epsilon);
  CHECK(perturbation.isApprox(recovered_perturbation, std::sqrt(epsilon)));

  const T recovered_identity = sym::LieGroupOps<T>::Retract(
    value, -recovered_perturbation, epsilon);
  CHECK(fabs(recovered_identity - identity) < std::sqrt(epsilon));

  const TangentVec perturbation_zero = sym::LieGroupOps<T>::LocalCoordinates(
    identity, recovered_identity, epsilon);
  CHECK(perturbation_zero.norm() < std::sqrt(epsilon));

  const T identity_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 0.0f, epsilon);
  CHECK(fabs(identity_interp - identity) < std::sqrt(epsilon));

  const T value_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 1.0f, epsilon);
  CHECK(fabs(value_interp - value) < std::sqrt(epsilon));
}

TEMPLATE_TEST_CASE("Test Matrix Lie group ops", "[geo_package]", {{ ", ".join(cpp_matrix_types) }}) {
  using T = TestType;

  using Scalar = typename sym::StorageOps<T>::Scalar;
  using TangentVec = Eigen::Matrix<Scalar, sym::LieGroupOps<T>::TangentDim(), 1>;
  const Scalar epsilon = sym::kDefaultEpsilon<Scalar>;

  const T identity = sym::GroupOps<T>::Identity();
  INFO("Testing Matrix LieGroupOps: " << identity.transpose());

  constexpr int32_t tangent_dim = sym::LieGroupOps<T>::TangentDim();
  CHECK(tangent_dim > 0);
  CHECK(tangent_dim <= sym::StorageOps<T>::StorageDim());

  std::mt19937 gen(42);
  const TangentVec perturbation = sym::Random<TangentVec>(gen);
  const T value = sym::LieGroupOps<T>::FromTangent(perturbation, epsilon);

  TestIsClose<T>::Test(gen);

  const TangentVec recovered_perturbation = sym::LieGroupOps<T>::ToTangent(value, epsilon);
  CHECK(perturbation.isApprox(recovered_perturbation, std::sqrt(epsilon)));

  const T recovered_identity = sym::LieGroupOps<T>::Retract(
    value, -recovered_perturbation, epsilon);
  CHECK(recovered_identity.isApprox(identity, std::sqrt(epsilon)));

  const TangentVec perturbation_zero = sym::LieGroupOps<T>::LocalCoordinates(
    identity, recovered_identity, epsilon);
  CHECK(perturbation_zero.norm() < std::sqrt(epsilon));

  const T identity_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 0.0f, epsilon);
  CHECK(identity_interp.isApprox(identity, std::sqrt(epsilon)));

  const T value_interp = sym::LieGroupOps<T>::Interpolate(identity, value, 1.0f, epsilon);
  CHECK(value_interp.isApprox(value, std::sqrt(epsilon)));
}
